#include<chrono>
#include <memory>
#include<functional>
#include<string>

#include "rclcpp/rclcpp.hpp"
#include "manipulation/msg/mani_process_cmd.hpp"

using namespace std::chrono_literals;

class MinimalPublisher : public rclcpp::Node
{
  public:
    MinimalPublisher()
    : Node("minimal_publisher"), count__(0)
    {
      publisher_ = this->create_publisher<manipulation::msg::ManiProcessCmd>("mani_process", 10);
      timer_ = this->create_wall_timer(
        500ms, std::bind(&MinimalPublisher::timer_callback, this));
    }
  private:
    void timer_callback()
    {
      auto message = manipulation::msg::ManiProcessCmd();
      message.value = 5.3;
      message.level1_name = "task0";
      message.level2_name = "head_down";
     
      RCLCPP_INFO(this->get_logger(), "Publishing: '%lf'", message.value);

       RCLCPP_INFO(this->get_logger(), "Publishing ManiProcessCmd: level1_name='%s', level2_name='%s'",
                message.level1_name.c_str(), message.level2_name.c_str());

      publisher_->publish(message);
    }
    rclcpp::Publisher<manipulation::msg::ManiProcessCmd>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
    size_t count__;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalPublisher>());
  rclcpp::shutdown();
  return 0;
}